/*=============================================================================
#    The element-comm is a tentative program used to test our ideas.           
#                       Copyright (C)  bgao                                    
#
#     This program is free software; you can redistribute it and/or            
#      modify it under the terms of the GNU General Public License             
#     as published by the Free Software Foundation; either version 2           
#         of the License, or (at your option) any later version.               
#
#     This program is distributed in the hope that it will be useful,          
#    but WITHOUT ANY WARRANTY; without even the implied warranty of           
#     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the            
#             GNU General Public License for more details.                    
#
#    You should have received a copy of the GNU General Public License        
#     along with this program; if not, write to the Free Software             
#      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,             
#                       MA  02110-1301, USA.                                  
#==============================================================================
# $Id$ 
#
# $Author$ 
#
# $Revision$ 
#
# $Date$ 
#
# Description: 
#
=============================================================================*/
#include    "rtos-wsn.h"

typedef struct {
	void (*tp) ();
} wsn_backtask_entry;

enum {
	WSN_MAX_BACKTASKS = 8,
	WSN_BACKTASK_BITMASK = (WSN_MAX_BACKTASKS - 1),
};

volatile static wsn_backtask_entry wsn_backtask_queue[WSN_MAX_BACKTASKS];
static uint8 wsn_sched_full;
volatile static uint8 wsn_sched_free;

void wsn_backtask_init(void)
{
	int i;
	wsn_sched_free = 0;
	wsn_sched_full = 0;
	for (i = 0; i < WSN_MAX_BACKTASKS; i++)
		wsn_backtask_queue[i].tp = NULL;
}


/*
 * wsn_backtask_post (function_pointer)
 *  
 * Put the task pointer into the next free slot.
 * Return 1 if successful, 0 if there is no free slot.
 *
 * This function uses a critical section to protect wsn_sched_free.
 * As tasks can be posted in both interrupt and non-interrupt context,
 * this is necessary.
 */
bool wsn_backtask_post(void (*tp) ())
{
	uint8 tmp;

	//  dbg(DBG_SCHED, ("wsn_post: %d 0x%x\n", wsn_sched_free, (int)tp));

	wsn_EnterCritical();

	tmp = wsn_sched_free;

	if (wsn_backtask_queue[tmp].tp == NULL) {
		wsn_sched_free = (tmp + 1) & WSN_BACKTASK_BITMASK;
		wsn_backtask_queue[tmp].tp = tp;
		wsn_LeaveCritical();

		return TRUE;
	}
	else {	
		wsn_LeaveCritical();
		
		/** backtask post fail **/
		WSN_debug(0x02cc, 0);
		return FALSE;
	}
}


/*
 * wsn_run_next_backtask()
 *
 * Remove the task at the head of the queue and execute it, freeing
 * the queue entry. Return 1 if a task was executed, 0 if the queue
 * is empty.
 */

bool wsn_run_next_backtask (void)
{
	uint8 old_full;
	void (*func)(void);

	wsn_EnterCritical();
	old_full = wsn_sched_full;
	func = wsn_backtask_queue[old_full].tp;
	if (func == NULL)
	{
		wsn_system_sleep();
		return 0;
	}

	wsn_backtask_queue[old_full].tp = NULL;
	wsn_sched_full = (old_full + 1) & WSN_BACKTASK_BITMASK;
	wsn_LeaveCritical();
	func();

	return 1;
}



